/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

//A little fudgery to do the equivalent of what the Arduino IDE does
//behind the scenes

#include "arduino_headers.h"
#include <EEPROM/EEPROM.cpp>
#include "module.h"


//Number of counts with zero CPU load from FlightOS
#define NOLOAD  283974

//define this and run to get the number to plug into NOLOAD
//#define CALCNOLOAD


#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)

void setup()
{

    /* Zero out all unused memory.
    This is used to track memory usage*/
    uint8_t *heapptr, *stackptr, *testptr;
    stackptr = (uint8_t *)malloc(4);          // use stackptr temporarily
    heapptr = stackptr;                     // save socket of heap pointer
    free(stackptr);      // free up the memory again (sets stackptr to 0)
    stackptr =  (uint8_t *)(SP);           // save socket of stack pointer
    while(heapptr < stackptr)
    {
        *heapptr++ = 0;
    }
#ifdef CALCNOLOAD
    Serial.begin(115200);
    return;
#endif

    g_moduleManager.Init();
    TCCR1A = 0;             // normal counting mode
    TCCR1B = _BV(CS11);     // set prescaler of 8
    TCNT1 = 0;              // clear the timer count
    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt
    OCR1A = usToTicks(1000); //trigger every 1ms
}




int g_cpuLoad = 100;

void loop()
{
    //this routine calculates approximate CPU load.
    //It is dependent on compiler settings and CPU clock.
    //If you change either, calibrate by defining CALCNOLOAD

    long time = micros();
    long ct = 0;
    do //see how may times we increment in 1 second
    {
        ct++;
    }while(micros() - time < 1000000);

#ifdef CALCNOLOAD
    Serial.print("#NOLOAD = ");
    Serial.println(ct);
    delay(100);
#else
    ct = (NOLOAD - ct) * 100;
    ct /= NOLOAD;
    if(ct <0)
    {
        ct = 0;
    }
    g_cpuLoad = ct;
#endif
}


int GetCPULoad()
{
    return(g_cpuLoad);
}

ISR (TIMER1_COMPA_vect, ISR_NOBLOCK)
{
    TCNT1 = 0;
#ifdef CALCNOLOAD
    return;
#endif
    ModuleManager::InterruptHandler();
}

